/*
	scheduler_isr.c

	Contains the various scheduler ISR sub-functions (called by the scheduler ISRs) required for operation and a few helper functions

	Author: Aidan Goddard 31/7/13
*/

#include"headers/types.h"
#include"headers/scheduler_tables.h"
#include"headers/kernel_tables.h"
#include"headers/scheduler_functions.h"
#include"headers/bakery_lock.h"

#include"../setup/headers/printf.h"

extern void ChangePML4(uint64_t new_PML4);

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// APIC scheduler functions

// scheduler entry modes
#define SCH_MODE_NEW_TASK_CURRENTLY_IDLE 1
#define SCH_MODE_NEW_TASK_NOT_CURRENTLY_IDLE 2
#define SCH_MODE_SCHEDULING_OFF_LOAD_IDLE 3
#define SCH_MODE_SLEEP_THIS_THREAD 4
#define SCH_MODE_KILL_THIS_THREAD 5
#define SCH_MODE_WAIT_THIS_THREAD 6
#define SCH_MODE_KILL_THIS_PROCESS 7

// main function (called by ISR)
SCH_TCB *APICSchedulerISR(uint64_t mode)
{
	// get CPU ID and to_return container (default to IDLE thread)
	SCH_TCB *to_return = (SCH_TCB*)(0x24b000 + 0x100b00000);
	int CPU = GetCPUID();

	// check mode
	if(mode == SCH_MODE_NEW_TASK_CURRENTLY_IDLE)
	{
		// currently idle, so do not need to shift the current thread
		// get lock on this queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// check active queue is not empty (it could happen)
		if(SCHEDULER->CPU_active_start[CPU] != NULL)
		{

			// active queue is not empty.
			// grab first entry
			SCH_TLC *thread = SCHEDULER->CPU_active_start[CPU];

			// remove it
			// check if end of queue also points to this thread
			if(SCHEDULER->CPU_active_end[CPU] == thread)
				SCHEDULER->CPU_active_end[CPU] = NULL;

			// remove the thread from the start of the queue
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
			thread->next_TLC = NULL;

			// put it in the current slot
			SCHEDULER->CPU_current_thread[CPU] = thread;

			// now get PML4 and TCB address for this thread
			int Pindex = thread->PCB_index;
			int Tindex = thread->TCB_index;
			uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
			to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

			// now change the PML4 if they are different
			ChangePML4(new_PML4);

		}

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;

	}
	else if(mode == SCH_MODE_NEW_TASK_NOT_CURRENTLY_IDLE)
	{
		//while(1){}
		// get lock on queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// move current thread to back of active queue (also check front of queue for valid pointer)
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];

		// check if queue is empty
		if(SCHEDULER->CPU_active_start[CPU] == NULL)
		{
			// queue is empty, move to start and end
			SCHEDULER->CPU_active_start[CPU] = thread;
			SCHEDULER->CPU_active_end[CPU] = thread;
			thread->next_TLC = NULL;
		}
		else
		{
			// queue has something in it, move this thread to the end of it
			SCHEDULER->CPU_active_end[CPU]->next_TLC = thread;
			SCHEDULER->CPU_active_end[CPU] = thread;
			thread->next_TLC = NULL;
		}

		// move front of active queue to the current thread
		// the queue is now DEFINITELY not empty
		// get thread at front of the queue
		thread = SCHEDULER->CPU_active_start[CPU];

		// check if start and end point to the same object
		if(SCHEDULER->CPU_active_end[CPU] == SCHEDULER->CPU_active_start[CPU])
		{
			// they point to the same object - the only one in the queue, null them both
			SCHEDULER->CPU_active_end[CPU] = NULL;
			SCHEDULER->CPU_active_start[CPU] = NULL;
		}
		else
		{
			// they point to different objects, make the start point to the next entry
			// the end can be left alone
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
		}

		// object is orphaned, stick it in the current thread list
		SCHEDULER->CPU_current_thread[CPU] = thread;
		thread->next_TLC = NULL;

		// now get PML4 and TCB address for this thread
		int Pindex = thread->PCB_index;
		int Tindex = thread->TCB_index;
		uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
		to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

		// now change the PML4 if they are different
		ChangePML4(new_PML4);

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;
	}
	else if(mode == SCH_MODE_SCHEDULING_OFF_LOAD_IDLE)
	{
		// get lock on queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// move current thread to back of active queue (also check front of queue for valid pointer)
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];

		// check if queue is empty
		if(SCHEDULER->CPU_active_start[CPU] == NULL)
		{
			// queue is empty, move to start and end
			SCHEDULER->CPU_active_start[CPU] = thread;
			SCHEDULER->CPU_active_end[CPU] = thread;
			thread->next_TLC = NULL;
		}
		else
		{
			// queue has something in it, move this thread to the end of it
			SCHEDULER->CPU_active_end[CPU]->next_TLC = thread;
			SCHEDULER->CPU_active_end[CPU] = thread;
			thread->next_TLC = NULL;
		}

		// set current thread to the idle thread
		SCHEDULER->CPU_current_thread[CPU] = NULL;

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to inactive
		SCHEDULER->CPU_current_state[CPU] = CPU_NOT_SCHEDULING;
	}
	else if (mode == SCH_MODE_SLEEP_THIS_THREAD)
	{
		// first get reference to the thread TLC and grab the total sleep counter
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];
		int Pindex = thread->PCB_index;
		int Tindex = thread->TCB_index;
		int sleep_counter = SCHEDULER->PCB[Pindex].TCB[Tindex].sleep_counter;

		// set current thread to NULL and reduce the CPU's active thread count
		if(SCHEDULER->CPU_active_thread_count[CPU] > 0)
		{
			SCHEDULER->CPU_active_thread_count[CPU]--;
		}
		SCHEDULER->CPU_current_thread[CPU] = NULL;

		// get lock on sleeping queue
		BakeryEnter(&(SCHEDULER->sleeping_lock), CPU);

		// check if the queue is empty
		if(SCHEDULER->sleeping_start == NULL)
		{
			// queue is empty
			// place the thread in it
			SCHEDULER->sleeping_start = thread;
			SCHEDULER->sleeping_end = thread;
			thread->sleep_offset = sleep_counter;
		}
		else
		{
			// queue is not empty, need to find the correct slot in it for this thread
			// set this thread's next thread pointer to null
			thread->next_TLC = NULL;

			// first check the first thread in the queue
			if(SCHEDULER->sleeping_start->sleep_offset < sleep_counter)
			{
				// first thread's sleep counter is smaller than this one, so it needs to be in front
				// subtract the first thread's sleep offset from the sleep counter and define a pointer to the previous thread
				sleep_counter -= SCHEDULER->sleeping_start->sleep_offset;
				SCH_TLC *prev_thread = SCHEDULER->sleeping_start;

				// iterate through the queue to find a suitable slot
				while(1)
				{
					// check if we are at the end of the queue (next thread is NULL)
					if(prev_thread->next_TLC == NULL)
					{
						// insert this thread at the end of the queue and finish
						prev_thread->next_TLC = thread;
						SCHEDULER->sleeping_end = thread;
						break;
					}

					// not at end of queue
					// check if the sleep counter is smaller than the next thread's sleep offset
					if(sleep_counter <= prev_thread->next_TLC->sleep_offset)
					{
						// it is smaller or equal, this thread can go before the next thread
						thread->next_TLC = prev_thread->next_TLC;
						prev_thread->next_TLC = thread;

						// update the next thread's sleep offset (if the counters were equal, this will be zeroed)
						thread->next_TLC->sleep_offset -= sleep_counter;

						// finished
						break;
					}

					// this means we are moving to the next thread in the queue
					// update the sleep counter and the prev_thread pointer
					sleep_counter -= prev_thread->next_TLC->sleep_offset;
					prev_thread = prev_thread->next_TLC;
				}

				// found it's place in the queue, update the sleep offset for this thread
				thread->sleep_offset = sleep_counter;

			}
			else
			{
				// first thread's sleep counter is larger than this one, so it needs to be behind.
				// subtract this thread's counter from the first thread's counter
				SCHEDULER->sleeping_start->sleep_offset -= sleep_counter;

				// set this thread as the first thread
				thread->next_TLC = SCHEDULER->sleeping_start;
				SCHEDULER->sleeping_start = thread;

				// set this thread's sleep offset
				thread->sleep_offset = sleep_counter;
			}
		}

		// release lock on sleeping queue
		BakeryLeave(&(SCHEDULER->sleeping_lock), CPU);

		// now need to check if there is another job available on this CPU
		// currently idle, so do not need to shift the current thread
		// get lock on this queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// check active queue is not empty (it could happen)
		if(SCHEDULER->CPU_active_start[CPU] != NULL)
		{

			// active queue is not empty.
			// grab first entry
			SCH_TLC *thread = SCHEDULER->CPU_active_start[CPU];

			// remove it
			// check if end of queue also points to this thread
			if(SCHEDULER->CPU_active_end[CPU] == thread)
				SCHEDULER->CPU_active_end[CPU] = NULL;

			// remove the thread from the start of the queue
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
			thread->next_TLC = NULL;

			// put it in the current slot
			SCHEDULER->CPU_current_thread[CPU] = thread;

			// now get PML4 and TCB address for this thread
			int Pindex = thread->PCB_index;
			int Tindex = thread->TCB_index;
			uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
			to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

			// now change the PML4 if they are different
			ChangePML4(new_PML4);
		}

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;
	}
	else if(mode == SCH_MODE_WAIT_THIS_THREAD)
	{
		// this thread needs to be moved to the wait queue
		// get the TLC object and null the current thread value
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];
		SCHEDULER->CPU_current_thread[CPU] = NULL;
		if(SCHEDULER->CPU_active_thread_count[CPU] > 0)
		{
			SCHEDULER->CPU_active_thread_count[CPU]--;
		}

		// get a lock on the waiting queue
		BakeryEnter(&(SCHEDULER->waiting_lock), CPU);

		// put this thread to the end of the queue
		// check if the queue is currently empty
		if(SCHEDULER->waiting_start == NULL)
		{
			// queue is currently empty, make both start and end point to this thread
			SCHEDULER->waiting_start = thread;
			SCHEDULER->waiting_end = thread;
			thread->next_TLC = NULL;
		}
		else
		{
			// queue is not empty
			// put it at the end
			SCHEDULER->waiting_end->next_TLC = thread;
			SCHEDULER->waiting_end = thread;
			thread->next_TLC = NULL;
		}


		// release waiting queue lock
		BakeryLeave(&(SCHEDULER->waiting_lock), CPU);

		// now need to check if there is another job available on this CPU
		// currently idle, so do not need to shift the current thread
		// get lock on this queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// check active queue is not empty (it could happen)
		if(SCHEDULER->CPU_active_start[CPU] != NULL)
		{

			// active queue is not empty.
			// grab first entry
			SCH_TLC *thread = SCHEDULER->CPU_active_start[CPU];

			// remove it
			// check if end of queue also points to this thread
			if(SCHEDULER->CPU_active_end[CPU] == thread)
				SCHEDULER->CPU_active_end[CPU] = NULL;

			// remove the thread from the start of the queue
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
			thread->next_TLC = NULL;

			// put it in the current slot
			SCHEDULER->CPU_current_thread[CPU] = thread;

			// now get PML4 and TCB address for this thread
			int Pindex = thread->PCB_index;
			int Tindex = thread->TCB_index;
			uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
			to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

			// now change the PML4 if they are different
			ChangePML4(new_PML4);
		}

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;

	}
	else if(mode == SCH_MODE_KILL_THIS_THREAD)
	{
		// this secondary thread thread needs killing
		// first move it from the current thread container
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];
		SCHEDULER->CPU_current_thread[CPU] = NULL;
		SCHEDULER->CPU_active_thread_count[CPU]--;

		// get Tindex and Pindex
		int Tindex = thread->TCB_index;
		int Pindex = thread->PCB_index;

		// get lock on the PCB
		BakeryEnter(&(SCHEDULER->PCB[Pindex].lock), CPU);

		// grab the TID to wake and this thread's return value from the TCB
		uint64_t TID_to_wake = SCHEDULER->PCB[Pindex].TCB[Tindex].TID_to_wake;
		uint64_t return_value = SCHEDULER->PCB[Pindex].TCB[Tindex].close_return_value;

		// set thread state to unused
		SCHEDULER->PCB[Pindex].PDB->thread_usage_map[Tindex] = THREAD_STATE_INACTIVE;

		// ditch the TLC
		thread->next_TLC = NULL;
		thread->PCB_index = 0;
		thread->TCB_index = 0;
		thread->sleep_offset = 0;
		PushTLC(thread);

		// reduce the process's thread count
		SCHEDULER->PCB[Pindex].active_threads--;

		// unlock the PCB
		BakeryLeave(&(SCHEDULER->PCB[Pindex].lock), CPU);

		// wake the thread that should be woken when this closes, pass it the return value from this thread's function
		if(TID_to_wake != 0)
		{
			WakeThread(TID_to_wake, return_value);
		}

		// select a new thread
		// now need to check if there is another job available on this CPU
		// currently idle, so do not need to shift the current thread
		// get lock on this queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// check active queue is not empty (it could happen)
		if(SCHEDULER->CPU_active_start[CPU] != NULL)
		{

			// active queue is not empty.
			// grab first entry
			SCH_TLC *thread = SCHEDULER->CPU_active_start[CPU];

			// remove it
			// check if end of queue also points to this thread
			if(SCHEDULER->CPU_active_end[CPU] == thread)
				SCHEDULER->CPU_active_end[CPU] = NULL;

			// remove the thread from the start of the queue
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
			thread->next_TLC = NULL;

			// put it in the current slot
			SCHEDULER->CPU_current_thread[CPU] = thread;

			// now get PML4 and TCB address for this thread
			int Pindex = thread->PCB_index;
			int Tindex = thread->TCB_index;
			uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
			to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

			// now change the PML4 if they are different
			ChangePML4(new_PML4);
		}

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;

	}
	else if(mode == SCH_MODE_KILL_THIS_PROCESS)
	{
		// this process is terminating...
		// this primary thread needs killing
		// first move it from the current thread container
		SCH_TLC *thread = SCHEDULER->CPU_current_thread[CPU];
		SCHEDULER->CPU_current_thread[CPU] = NULL;
		SCHEDULER->CPU_active_thread_count[CPU]--;

		// get Tindex and Pindex
		int Tindex = thread->TCB_index;
		int Pindex = thread->PCB_index;

		// get lock on the PCB
		BakeryEnter(&(SCHEDULER->PCB[Pindex].lock), CPU);

		// grab the TID to wake and this thread's return value from the TCB
		uint64_t TID_to_wake = SCHEDULER->PCB[Pindex].TCB[Tindex].TID_to_wake;
		uint64_t return_value = SCHEDULER->PCB[Pindex].TCB[Tindex].close_return_value;

		// set thread state to unused
		SCHEDULER->PCB[Pindex].PDB->thread_usage_map[Tindex] = THREAD_STATE_INACTIVE;

		// ditch the TLC
		thread->next_TLC = NULL;
		thread->PCB_index = 0;
		thread->TCB_index = 0;
		thread->sleep_offset = 0;
		PushTLC(thread);

		// reduce the process's thread count
		SCHEDULER->PCB[Pindex].active_threads--;

		// set process state to terminated
		SCHEDULER->PCB[Pindex].state = PROCESS_STATE_TERMINATED;

		// unlock the PCB
		BakeryLeave(&(SCHEDULER->PCB[Pindex].lock), CPU);

		// wake the thread that should be woken when this closes, pass it the return value from this thread's function
		if(TID_to_wake != 0)
		{
			WakeThread(TID_to_wake, return_value);
		}

		// select a new thread
		// now need to check if there is another job available on this CPU
		// currently idle, so do not need to shift the current thread
		// get lock on this queue
		BakeryEnter(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// check active queue is not empty (it could happen)
		if(SCHEDULER->CPU_active_start[CPU] != NULL)
		{

			// active queue is not empty.
			// grab first entry
			SCH_TLC *thread = SCHEDULER->CPU_active_start[CPU];

			// remove it
			// check if end of queue also points to this thread
			if(SCHEDULER->CPU_active_end[CPU] == thread)
				SCHEDULER->CPU_active_end[CPU] = NULL;

			// remove the thread from the start of the queue
			SCHEDULER->CPU_active_start[CPU] = thread->next_TLC;
			thread->next_TLC = NULL;

			// put it in the current slot
			SCHEDULER->CPU_current_thread[CPU] = thread;

			// now get PML4 and TCB address for this thread
			int Pindex = thread->PCB_index;
			int Tindex = thread->TCB_index;
			uint64_t new_PML4 = SCHEDULER->PCB[Pindex].PML4;
			to_return = &(SCHEDULER->PCB[Pindex].TCB[Tindex]);

			// now change the PML4 if they are different
			ChangePML4(new_PML4);
		}

		// release lock
		BakeryLeave(&(SCHEDULER->CPU_active_lock[CPU]), CPU);

		// set cpu state to active
		SCHEDULER->CPU_current_state[CPU] = CPU_SCHEDULING;
	}
	else
	{
		// something went HORRIBLY wrong here? halt system scheduler.
		// set cpu state to inactive
		SCHEDULER->CPU_current_state[CPU] = CPU_NOT_SCHEDULING;
		SCHEDULER->scheduling_state = SCHEDULING_STATE_DISABLED;

		// wait for all other CPUs to return not scheduling
		int i;
		for(i = 0; i < SCHEDULER->active_CPU_count; i++)
		{
			while(SCHEDULER->CPU_current_state[i] == CPU_SCHEDULING){}
		}

		// TODO: change to invoke the kernel panic handler
		__asm__ __volatile__("cli; hlt;":::"memory");
	}

	// return selected thread structure
	return to_return;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// HPET scheduler sleep counter

void HPETSleepCounterISR(void)
{
	// this is only called if the thread sleeping queue is NOT empty
	// should only be called by CPU0 (BSP)
	// get lock on sleeping thread queue
	BakeryEnter(&(SCHEDULER->sleeping_lock), 0);
	//printf("\nisr entered : ");

	// get pointer to the thread container at the front of the queue
	SCH_TLC *front_thread = SCHEDULER->sleeping_start;

	// decrease its counter
	front_thread->sleep_offset -= 1000; // 1000 micro-seconds per ISR
	//printf("front counter %u", front_thread->sleep_offset);

	// check counter value for zero value
	if(front_thread->sleep_offset > 0)
	{
		// release lock and leave
		BakeryLeave(&(SCHEDULER->sleeping_lock), 0);
		return;
	}

	// counter has reached 0, loop until all threads with offset 0 are woken
	while(1)
	{
		// counter has reached zero, need to move it to a suitable CPU
		int lowest_cpu = GetCPUWithSmallestActiveQueue();

		// move the thread to that CPU
		SCHEDULER->sleeping_start = front_thread->next_TLC;
		if(SCHEDULER->sleeping_start == NULL)
		{
			SCHEDULER->sleeping_end = NULL;
		}

		//int cpu_new_thread_count = MoveThreadToCPU(lowest_cpu, front_thread);
		MoveThreadToCPU(lowest_cpu, front_thread);

		// TODO: send IPI to CPU to make it enter the scheduler immediately
		// have CPU ID in lowest_cpu
		// get APIC ID from system config tables
		TBL_SYSCONFIG *config_base = GetSystemConfigBase();
		uint32_t APIC_ID = config_base->tbl_cpu[lowest_cpu].APIC_ID;

		// get base address of this APIC
		uint32_t apic_low, apic_high;
		rdmsr(0x1b, &apic_low, &apic_high);		// get msr contents
		volatile uint32_t *APIC_base = (uint32_t*)(uint64_t)(apic_low & 0xfffff000);

		// send FIXED IPI to that CPU on vector 50
		uint32_t command = 50;
		APIC_ID = APIC_ID << 24;
		__asm__ __volatile__("mfence":::"memory");
		APIC_base[0xc4] = APIC_ID;
		__asm__ __volatile__("mfence":::"memory");
		APIC_base[0xc0] = command;
		__asm__ __volatile__("mfence":::"memory");

		// get new front thread
		front_thread = SCHEDULER->sleeping_start;

		// check for null value
		if(front_thread == NULL)
		{
			break;
		}

		// check new front of thread for 0 (no offset)
		if(front_thread->sleep_offset > 0)
		{
			// new front thread has to wait, leave
			break;
		}

	}

	// release lock and leave
	BakeryLeave(&(SCHEDULER->sleeping_lock), 0);

	// finished, return to top ISR
	return;
}
